package com.yuneec.fly.utils;

import com.yuneec.fly.GlobalParams;
import com.yuneec.fly.views.AbstractMapView;

import java.util.ArrayList;

/**
 * Created by jw.gu on 2016/9/21 0021.
 */
public class Hermite {
    private static class Params {
        AbstractMapView.Vector2 params1;
        AbstractMapView.Vector2 params2;
        AbstractMapView.Vector2 params3;
        AbstractMapView.Vector2 params4;

        public Params() {
            params3 = new AbstractMapView.Vector2();
            params4 = new AbstractMapView.Vector2();
        }
    }

    private static Params getParams(AbstractMapView.Vector2 origin, AbstractMapView.Vector2 dest, AbstractMapView.Vector2 originVel, AbstractMapView.Vector2 destVel) {
        Params params = new Params();
        params.params1 = origin;
        params.params2 = originVel;
        params.params3.x = -origin.x * 3 - originVel.x * 2 + dest.x * 3 - destVel.x;
        params.params3.y = -origin.y * 3 - originVel.y * 2 + dest.y * 3 - destVel.y;
        params.params4.x = origin.x * 2 + originVel.x - dest.x * 2 + destVel.x;
        params.params4.y = origin.y * 2 + originVel.y - dest.y * 2 + destVel.y;
        return params;
    }

    private static AbstractMapView.Vector2 getNextPoint(float progress, Params params) {
        AbstractMapView.Vector2 result = new AbstractMapView.Vector2();
        float progressSqrd = progress * progress;
        float progressCubed = progressSqrd * progress;
        result.x = params.params1.x + params.params2.x * progress + params.params3.x * progressSqrd + params.params4.x * progressCubed;
        result.y = params.params1.y + params.params2.y * progress + params.params3.y * progressSqrd + params.params4.y * progressCubed;
        return result;
    }

    public static ArrayList<AbstractMapView.Vector2> getCurveRouteVector2s(int startIndex, int endIndex) {//index start from 1
        ArrayList<AbstractMapView.Vector2> vector2s = new ArrayList<>();
//        if(endIndex<2){
//            return vector2s;
//        }
//        if(endIndex-startIndex==1){
//            vector2s.add(new AbstractMapView.Vector2(GlobalParams.waypoints.get(startIndex-1).latitude,GlobalParams.waypoints.get(startIndex-1).longitude));
//        }
//        for (int i=startIndex;i<endIndex-1;i++){
//            Waypoint startPoint= GlobalParams.waypoints.get(i-1);
//            Waypoint endPoint= GlobalParams.waypoints.get(i);
//            Waypoint nextPoint;
//            AbstractMapView.Vector2 start=new AbstractMapView.Vector2(startPoint.latitude,startPoint.longitude);
//            AbstractMapView.Vector2 end=new AbstractMapView.Vector2(endPoint.latitude,endPoint.longitude);
//            AbstractMapView.Vector2 startVel=new AbstractMapView.Vector2();
//            AbstractMapView.Vector2 endVel=new AbstractMapView.Vector2();
//
//            float d = LocationUtils.getDistanceBetweenTwoPoint(startPoint.latitude, startPoint.longitude, endPoint.latitude, endPoint.longitude);
//            float count= (int) (d/5);
//            count=count>=20?count:20;
//            count=count<=30?count:30;
//            vector2s.add(start);
//            startVel.x=end.x-start.x;
//            startVel.y=end.y-start.y;
////            if(i<endIndex-2){
//            nextPoint=GlobalParams.waypoints.get(i+1);
////            }else{
////                nextPoint=GlobalParams.waypoints.get(startIndex-1);
////            }
//            endVel.x=nextPoint.latitude-endPoint.latitude;
//            endVel.y=nextPoint.longitude-endPoint.longitude;
//
//            Params params= getParams(start,end,startVel,endVel);
//            for (int j=1;j<count;j++){
//                vector2s.add(getNextPoint(j / count, params));
//            }
//        }
//        vector2s.add(new AbstractMapView.Vector2(GlobalParams.waypoints.get(endIndex-1).latitude,GlobalParams.waypoints.get(endIndex-1).longitude));
        return vector2s;
    }
}
